;
; Ephi - simulation of magnetic fields and particles
; Copyright (C) 2007 Indrek Mandre <indrek(at)mare.ee>
; For more information please see http://www.mare.ee/indrek/ephi/
;
; This program is free software; you can redistribute it and/or modify
; it under the terms of the GNU General Public License as published by
; the Free Software Foundation; either verdxon 2 of the License, or
; (at your option) any later verdxon.
;
; This program is distributed in the hope that it will be useful,
; but WITHOUT ANY WARRANTY; without even the implied warranty of
; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
; GNU General Public License for more details.
;
; You should have received a copy of the GNU General Public License along
; with this program; if not, write to the Free Software Foundation, Inc.,
; 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
;

BITS 64

global addbfield_sse_double
global addbfield_sse_float

; extern "C" void addbfield_sse_double (double *posv, double *kmiv, double *begin, double *end, double *resbuf)
addbfield_sse_double:
    ; posv=rdi, kmiv=rsi, begin=rdx, end=rcx, resbuf=r8

    movapd xmm8, [r8]
    movapd xmm9, [r8 + 16]
    movapd xmm10, [r8 + 32]

    ; sanity check that we are not passed an empty dataset
    cmp rdx, rcx
    jne .loop
    ret

.loop:
    ;;;;;;;;;;;;;;;;;;;;;;;; step1: load pos and middle
    ;; load x0; // x0 = p.x
    ;; load x1; // x1 = p.y
    ;; load x2; // x2 = p.z
    ;;
    ;; load x3; // x3 = m.x
    ;; load x4; // x4 = m.y
    ;; load x5; // x5 = m.z
    movapd xmm0, [rdi]
    movapd xmm3, [rdx]

    movapd xmm1, [rdi + 16]
    movapd xmm4, [rdx + 16]

    movapd xmm2, [rdi + 32]
    movapd xmm5, [rdx + 32]

    ;;;;;;;;;;;;;;;;;;;;;;;; step2: find vector r
    ;; x3 = x3 - x0; // x3 = m.x - p.x (r.x)
    ;; x4 = x4 - x1; // x4 = m.y - p.y (r.y)
    ;; x5 = x5 - x2; // x5 = m.z - p.z (r.z)
    subpd xmm3, xmm0
    subpd xmm4, xmm1
    subpd xmm5, xmm2

    ;;;;;;;;;;;;;;;;;;;;;;;; step3: find the indiviaul multiplication elements of the r vectors
    ;; x0 = x3; x0 = x0 * x0; // x0 = r.x * r.x
    ;; x1 = x4; x1 = x1 * x1; // x1 = r.y * r.y
    ;; x2 = x5; x2 = x2 * x2; // x2 = r.z * r.z
    movapd xmm0, xmm3
    mulpd xmm0, xmm0
    movapd xmm1, xmm4
    mulpd xmm1, xmm1
    movapd xmm2, xmm5
    mulpd xmm2, xmm2

    ;;;;;;;;;;;;;;;;;;;;;;;; step4: add up the multipliation elements to get the dot product value
    ;; x0 = x0 + x1; // x0 = r.x * r.x + r.y * r.y
    ;; x0 = x0 + x2; // x0 = r.x * r.x + r.y * r.y + r.z * r.z
    addpd xmm0, xmm1
    addpd xmm0, xmm2

    ;;;;;;;;;;;;;;;;;;;;;;;; step5: calculate the length of r
    ;; x1 = x0; x1 = sqrt(x1); // x1 = |r|
    sqrtpd xmm1, xmm0

    ;;;;;;;;;;;;;;;;;;;;;;;; step6: get |r|^3
    ;; x0 = x0 * x1; // x0 = |r|^3
    mulpd xmm0, xmm1

    ;;;;;;;;;;;;;;;;;;;;;;;; step7: load KmI
    ;; load x6; // x6 = (KmI, KmI)
    movapd xmm6, [rsi]

    ;;;;;;;;;;;;;;;;;;;;;;;; step8: divide KmI with the |r|^3 to find the scalar
    ;; x6 = x6 / x0; // x6 = mod
    divpd xmm6, xmm0

    ;;;;;;;;;;;;;;;;;;;;;;;; step9: mul modulator with the r vector
    ;; x3 = x3 * x6
    ;; x4 = x4 * x6
    ;; x5 = x5 * x6
    mulpd xmm3, xmm6
    mulpd xmm4, xmm6
    mulpd xmm5, xmm6

    ;;;;;;;;;;;;;;;;;;;;;;;; step 10: load slope
    ;; load x0; // x0 = s.x
    ;; load x1; // x1 = s.y
    ;; load x2; // x2 = s.z
    movapd xmm0, [rdx + 48]
    movapd xmm1, [rdx + 64]
    movapd xmm2, [rdx + 80]

    ;;;;;;;;;;;;;;;;;;;;;;;; step 11: the cross product
    ;; x6 = x1 * x5; // x6 = s.y * r.z
    ;; x7 = x2 * x4; // x7 = s.z * r.y
    ;; x7 = x7 - x6; // x7 = s.z * r.y - s.y * r.z = R.x
    ;;
    ;; x2 = x2 * x3; // x2 = s.z * r.x
    ;; x5 = x5 * x0; // x5 = s.x * r.z
    ;; x5 = x5 - x2; // x5 = s.x * r.z - s.z * r.x = R.y
    ;;
    ;; x0 = x0 * x4; // x0 = s.x * r.y
    ;; x1 = x1 * x3; // x1 = s.y * r.x
    ;; x1 = x1 - x0; // x1 = s.y * r.x - s.x * r.y = R.z
    movapd xmm6, xmm1
    mulpd xmm6, xmm5
    movapd xmm7, xmm2
    mulpd xmm7, xmm4
    subpd xmm7, xmm6

    mulpd xmm2, xmm3
    mulpd xmm5, xmm0
    subpd xmm5, xmm2

    mulpd xmm0, xmm4
    mulpd xmm1, xmm3
    subpd xmm1, xmm0

    ;; result pairs are now in (xmm7, xmm5, xmm1)
    addpd xmm8, xmm7
    addpd xmm9, xmm5
    addpd xmm10, xmm1

    ; moving to the next iteration
    add rdx, 96 ; is it ok to modify rdx whtout restoring in x86_64?
    cmp rdx, rcx
    jne .loop

    movapd [r8], xmm8
    movapd [r8 + 16], xmm9
    movapd [r8 + 32], xmm10

    ret

; extern "C" void addbfield_sse_float (float *posv, float *kmiv, float *begin, float *end, float *resbuf)
addbfield_sse_float:
    ; posv=rdi, kmiv=rsi, begin=rdx, end=rcx, resbuf=r8

    movaps xmm8, [r8]
    movaps xmm9, [r8 + 16]
    movaps xmm10, [r8 + 32]

    ; sanity check that we are not passed an empty dataset
    cmp rdx, rcx
    jne .loop
    ret

.loop:
    ;;;;;;;;;;;;;;;;;;;;;;;; step1: load pos and middle
    ;; load x0; // x0 = p.x
    ;; load x1; // x1 = p.y
    ;; load x2; // x2 = p.z
    ;;
    ;; load x3; // x3 = m.x
    ;; load x4; // x4 = m.y
    ;; load x5; // x5 = m.z
    movaps xmm0, [rdi]
    movaps xmm3, [rdx]

    movaps xmm1, [rdi + 16]
    movaps xmm4, [rdx + 16]

    movaps xmm2, [rdi + 32]
    movaps xmm5, [rdx + 32]

    ;;;;;;;;;;;;;;;;;;;;;;;; step2: find vector r
    ;; x3 = x3 - x0; // x3 = m.x - p.x (r.x)
    ;; x4 = x4 - x1; // x4 = m.y - p.y (r.y)
    ;; x5 = x5 - x2; // x5 = m.z - p.z (r.z)
    subps xmm3, xmm0
    subps xmm4, xmm1
    subps xmm5, xmm2

    ;;;;;;;;;;;;;;;;;;;;;;;; step3: find the indiviaul multiplication elements of the r vectors
    ;; x0 = x3; x0 = x0 * x0; // x0 = r.x * r.x
    ;; x1 = x4; x1 = x1 * x1; // x1 = r.y * r.y
    ;; x2 = x5; x2 = x2 * x2; // x2 = r.z * r.z
    movaps xmm0, xmm3
    mulps xmm0, xmm0
    movaps xmm1, xmm4
    mulps xmm1, xmm1
    movaps xmm2, xmm5
    mulps xmm2, xmm2

    ;;;;;;;;;;;;;;;;;;;;;;;; step4: add up the multipliation elements to get the dot product value
    ;; x0 = x0 + x1; // x0 = r.x * r.x + r.y * r.y
    ;; x0 = x0 + x2; // x0 = r.x * r.x + r.y * r.y + r.z * r.z
    addps xmm0, xmm1
    addps xmm0, xmm2

    ;;;;;;;;;;;;;;;;;;;;;;;; step5: calculate the length of r
    ;; x1 = x0; x1 = sqrt(x1); // x1 = |r|
    sqrtps xmm1, xmm0

    ;;;;;;;;;;;;;;;;;;;;;;;; step6: get |r|^3
    ;; x0 = x0 * x1; // x0 = |r|^3
    mulps xmm0, xmm1

    ;;;;;;;;;;;;;;;;;;;;;;;; step7: load KmI
    ;; load x6; // x6 = (KmI, KmI)
    movaps xmm6, [rsi]

    ;;;;;;;;;;;;;;;;;;;;;;;; step8: divide KmI with the |r|^3 to find the scalar
    ;; x6 = x6 / x0; // x6 = mod
    divps xmm6, xmm0

    ;;;;;;;;;;;;;;;;;;;;;;;; step9: mul modulator with the r vector
    ;; x3 = x3 * x6
    ;; x4 = x4 * x6
    ;; x5 = x5 * x6
    mulps xmm3, xmm6
    mulps xmm4, xmm6
    mulps xmm5, xmm6

    ;;;;;;;;;;;;;;;;;;;;;;;; step 10: load slope
    ;; load x0; // x0 = s.x
    ;; load x1; // x1 = s.y
    ;; load x2; // x2 = s.z
    movaps xmm0, [rdx + 48]
    movaps xmm1, [rdx + 64]
    movaps xmm2, [rdx + 80]

    ;;;;;;;;;;;;;;;;;;;;;;;; step 11: the cross product
    ;; x6 = x1 * x5; // x6 = s.y * r.z
    ;; x7 = x2 * x4; // x7 = s.z * r.y
    ;; x7 = x7 - x6; // x7 = s.z * r.y - s.y * r.z = R.x
    ;;
    ;; x2 = x2 * x3; // x2 = s.z * r.x
    ;; x5 = x5 * x0; // x5 = s.x * r.z
    ;; x5 = x5 - x2; // x5 = s.x * r.z - s.z * r.x = R.y
    ;;
    ;; x0 = x0 * x4; // x0 = s.x * r.y
    ;; x1 = x1 * x3; // x1 = s.y * r.x
    ;; x1 = x1 - x0; // x1 = s.y * r.x - s.x * r.y = R.z
    movaps xmm6, xmm1
    mulps xmm6, xmm5
    movaps xmm7, xmm2
    mulps xmm7, xmm4
    subps xmm7, xmm6

    mulps xmm2, xmm3
    mulps xmm5, xmm0
    subps xmm5, xmm2

    mulps xmm0, xmm4
    mulps xmm1, xmm3
    subps xmm1, xmm0

    ;; result pairs are now in (xmm7, xmm5, xmm1)
    addps xmm8, xmm7
    addps xmm9, xmm5
    addps xmm10, xmm1

    ; moving to the next iteration
    add rdx, 96
    cmp rdx, rcx
    jne .loop

    movaps [r8], xmm8
    movaps [r8 + 16], xmm9
    movaps [r8 + 32], xmm10

    ret

